from numpy import *

class SpaceCurve:
    def __init__(self,t):
        self.t = t
    def initial(self,pts):
        self.curve_size = len(self.t)
        self.curve_data = zeros((self.curve_size,3))
        for i in arange(self.curve_size):
            self.curve_data[i,:] = pts[i]
        self.curve_fit()
    def curve_fit(self):
        from scipy.interpolate import CubicSpline
        curve_data = self.curve_data
        xpath=curve_data[:,0]
        ypath=curve_data[:,1]
        zpath=curve_data[:,2]
        fx=CubicSpline(self.t,xpath)
        fy=CubicSpline(self.t,ypath)
        fz=CubicSpline(self.t,zpath)
        self.curve = (fx,fy,fz)
        return self.curve
    def curve_xyz(self,s,nu=0):
        x = self.curve[0](s,nu)
        y = self.curve[1](s,nu)
        z = self.curve[2](s,nu)
        return array([x,y,z])
    def curve_search_errors(self,para,xyz0):
        s=para
        x0,y0,z0=xyz0
        x = self.curve[0](s)
        y = self.curve[1](s)
        z = self.curve[2](s)
        return ((x-x0)**2.0+(y-y0)**2.0+(z-z0)**2.0)
    def search(self,pts):
        import scipy.optimize as opt
        tparams=opt.leastsq(self.curve_search_errors,0.0,pts,full_output=1)
        paras = tparams[0]
        return paras